package collision;

import geometry.Drawable;
import geometry.TriangleMesh;

import java.util.Comparator;
import java.util.Map;
import java.util.TreeMap;

import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import util.Constants;
import util.Timing;


public class ASDFOctree
{
	private ASDFOctreeNode root;
    public OBBTree obbtree;
    
    private PointComparator comparator = new PointComparator();
    private TreeMap<Point3d, Double> cachedDistance = new TreeMap<Point3d, Double>(comparator);
    
    private class PointComparator implements Comparator<Point3d>
    {
		public int compare(Point3d p0, Point3d p1)
		{
			if (p0.x < p1.x - Constants.FP_TOLERANCE)
				return -1;
			else if (p0.x > p1.x + Constants.FP_TOLERANCE)
				return 1;
			
			if (p0.y < p1.y - Constants.FP_TOLERANCE)
				return -1;
			else if (p0.y > p1.y + Constants.FP_TOLERANCE)
				return 1;
			
			if (p0.z < p1.z - Constants.FP_TOLERANCE)
				return -1;
			else if (p0.z > p1.z + Constants.FP_TOLERANCE)
				return 1;
			
			return 0;
		}
    }
    
    public double findCachedDistance(Point3d p)
    {
    	Map.Entry<Point3d, Double> entry = cachedDistance.ceilingEntry(p);
    	if (entry == null) return Double.MAX_VALUE;
    	
    	if (comparator.compare(entry.getKey(), p) == 0)
    		return entry.getValue();
    	else
    		return Double.MAX_VALUE;
    }
    
    public boolean addCachedDistance(Point3d p, double value)
    {
    	Map.Entry<Point3d, Double> entry = cachedDistance.ceilingEntry(p);
    	if (entry == null || comparator.compare(entry.getKey(), p) != 0)
    	{
    		cachedDistance.put(p, value);
    		return true;
    	}
    	else return false;
    }

    public ASDFOctree(Drawable geo_, OBBTree obbtree_)
    {
    	obbtree = obbtree_;
		BoundingBox bBox = geo_.getBoundingBox();
        Point3d minPt = new Point3d(bBox._minPt);
        Point3d maxPt = new Point3d(bBox._maxPt);
        System.out.println("minPt " + minPt);
        System.out.println("maxPt " + maxPt);
        root = new ASDFOctreeNode(this, null, minPt, maxPt);
    }

    /**
     * Get the distance to this ASDF. The answer will be correct ONLY when it's negative.
     * We don't need positive distance values for collision detection.
     */
    public double getDistance(Point3d pt_, Vector3d normal)
    {
        if (root.containsPoint(pt_) == false)
        	return Double.POSITIVE_INFINITY;
        
        long time1 = System.nanoTime();

        double ret = root.getDistance(pt_, normal);
        
        Timing.ASDFGetDistanceCount++;
        Timing.ASDFGetDistanceTime += System.nanoTime() - time1;
        
        return ret;
    }
}
